/*
 * Compass.cpp
 *
 *  Created on: 28/04/2013
 *      Author: roger
 */

#include "Compass.h"

static char *compass_bitmap PROGMEM =
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\"!1%%2%2!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\"Q=(/TM\\!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!#!A);'BI!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!\"Q=(F:76!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\"Q=(Q]0$!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!'\"A95V.4$!Q-\\?XN6&25)#!A!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!,#QML[_P_@HZ[OLK!Q-$^04U\\_`P`0T]P<W^/TM\\!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!:76F^P<W````````"
		"ML+S!!!!]@(RZ?8F````````_`P\\@X^`!1%\"!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!:76F`P\\_````R-4%0DY_#!A)!!!!\\O\\OE*#1"
		"-4%RM\\/T````````AY/$!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!,#QM^`@X````I+#A#1E*!!!!\"Q=(!!!!\\O\\OP,S]!!!!!Q-$B97&````"
		"`0T]2E:'!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!L[_P````"
		"R=8&#!A)86V>9G*C#QM,!!!!\\O\\O[_PL#1E*6F:7$Q]0KKKK````TM\\/!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!'\"A9_PL[````0DY_!!!!\"Q=(@X^`"
		"#AI+!!!!\\O\\O````?(BY@HZ_!!!!)S-D_PL[````.$1U!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!56&2]0$QZ/4E=H*SH:W>T-T-^@8VB97&!!!!]P,SI+#A"
		"#QM,$1U.#!A)#!A)GJK;Z?8F=H*S!!!!!!!!!!!!!!!!!!!!\"A9',#QM76F:BY?("
		"M\\/TZ?8F_`P\\````````````````````````B97&EZ/4\"15&!!!!!!!!!!!!!!!!"
		"!!!!!1%\"#QM,\"Q=(#!A)\"Q=(\"Q=(\"!1%$Q]0&B97&B97&R=8&B97&B97&R=8&\"15"
		"%\"!1%\"!1%\"!1%2%2%R-4>86VBI;']@(R\\_`P\\O\\O\\O\\O\\O\\O]@(R^04U]0$QS-D)"
		"GZO<<7VN1E*#&B97!!!!!!!!!!!!!!!!!!!!5V.4T-T-EZ/4#!A)\"A9'\"Q=(!1%\""
		":'2E_`P\\%R-4A9'\"````\\_`PQM,#FJ;7V^@8\\_`P=8&R!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!(R]@`0T]````-4%R!!!!5V.4C)C)^@8V````%2%2!!!!"
		"A9'\"#AI+!!!!'\"A9^`@X````04U^!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!P<W^````M\\/T!Q-$=(\"Q#QM,U>(2````%\"!1\"A9'=(\"Q<'RM!Q-$F:76"
		"````WNL;\"15&!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!/TM\\_`P\\"
		"````BY?(#!A)!!!!I[/D````%\"!1\"Q=(!!!!#AI+<'RM`@X^````7&B9!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!@X^`````````KKKK)C)C"
		">X>X````%\"!1#!A)'2E:F*35`@X^````HJ[?!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1%\"AY/$`0T]````_PL[U^04````&\"15@HZ_"
		"^`@X````````HJ[?\"Q=(!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!2E:'TM\\/````]0$Q````&B97R]@(````WNL;7&B9!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!.$1U<W^P[OLK&B97<W^P04U^\"15&!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!OLK[&B97!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!D9W.&B97!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!8FZ?&B97!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!-D)S&B97!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"!!!!#!A)%2%2!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
		"";

Compass::Compass(): App() {
	// Keep the constructor empty and do most things in the init()
}
void Compass::init() {
#ifdef COMPASS_DEBUG
	Serial.print(getName());Serial.println(INIT);
#endif
	m_icon = new Icon(28,&compass_bitmap[0]);
	m_height = Graphics.height();
	m_width = Graphics.width();
	m_cy = Graphics.height() / 2;
	m_cx = Graphics.width() / 2;
	m_cradius = min(m_cy,m_cx) * CIRCLE_PORTION;

	m_last.x = -1;
	m_last.y = -1;
	assert(m_last.y == -1);
	m_arrowSide = ARROW_LENGTH * cos(ARROW_WIDTH);
}
void Compass::setup() {
	Graphics.fillScreen(BLACK);
	Graphics.setRotation(0);
	Graphics.setCursor(0,0);
	m_height = Graphics.height();
	m_width = Graphics.width();
	m_cy = Graphics.height() / 2;
	m_cx = Graphics.width() / 2;
	m_cradius = min(m_cy,m_cx) * CIRCLE_PORTION;
	m_cycleCount = 10;
	Hardware.compass(true);
}
void Compass::close() {
	Hardware.compass(false);
}
void Compass::display() {
	int heading = Hardware.compassHeading();
	int correctedHeading = heading + ANGLE_CORRECTION;
	double heading_radians = correctedHeading * PI / 180;
	if (m_cycleCount++ > 5) {
		m_cycleCount = 0;
		Graphics.fillScreen(BLACK);
		m_last.x = -1;
		m_last.y = -1;
	}
	if (m_last.x != -1 && m_last.y != -1) {
		arrow(m_last.x,m_last.y,last_heading_radians,BLACK);
	}
	Graphics.drawCircle(m_cx, m_cy, m_cradius, WHITE);
	double dy = ((double) m_cradius) * cos(heading_radians);
	double dx = ((double) m_cradius) * sin(heading_radians);
	m_last.x = m_cx + dx;
	m_last.y = m_cy + dy;
	last_heading_radians = heading_radians+PI;

	Graphics.drawLine(0,m_cy,Graphics.width(),m_cy,WHITE );
	Graphics.drawLine(m_cx,0,m_cx,Graphics.height(),WHITE );
	arrow(m_last.x,m_last.y,last_heading_radians,YELLOW);
	int h = heading-150;
	h = (h > 180) ? ((180 - h) + 180) * -1 : h;
	displayHeading(h);
}

void Compass::arrow(uint16_t x, uint16_t y, double angle_radians,
		uint16_t colour) {
	float tx = (m_arrowSide * sin(angle_radians + ARROW_WIDTH)) + x;
	float ty = (m_arrowSide * cos(angle_radians + ARROW_WIDTH)) + y;
	float bx = (m_arrowSide * sin(angle_radians - ARROW_WIDTH)) + x;
	float by = (m_arrowSide * cos(angle_radians - ARROW_WIDTH)) + y;
	Graphics.fillTriangle(x, y, tx, ty, bx, by, colour);
}

int Compass::displayHeading(int h)
{
	Graphics.constructor(m_width,m_height); // Need this or the screen freezes.
	Graphics.fillCircle(m_cx, m_cy, m_cradius-ARROW_LENGTH, BLACK);
	Graphics.setRotation(3);
	Graphics.setCursor(m_cy-(m_cradius/3), m_cx-5);
	Graphics.setTextColor(WHITE);
	Graphics.setTextSize(2);
	Graphics.println(h);
	Graphics.setRotation(0);
	return h;
}

Compass::~Compass() {
}

App *compass = new Compass();


